/*
 *  P2OS for ROS
 *  Copyright (C) 2009
 *     David Feil-Seifer, Brian Gerkey, Kasper Stoy,
 *      Richard Vaughan, & Andrew Howard
 *
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/*
 * robot_params.h
 *
 * ActivMedia robot parameters, automatically generated by saphconv.tcl from
 * Saphira parameter files:
 *    amigo.p
 *    amigo-sh.p
 *    p2at.p
 *    p2at8+.p
 *    p2at8.p
 *    p2ce.p
 *    p2d8+.p
 *    p2d8.p
 *    p2de.p
 *    p2df.p
 *    p2dx.p
 *    p2it.p
 *    p2pb.p
 *    p2pp.p
 *    p3at-sh.p
 *    p3at.p
 *    p3atiw.p
 *    p3dx-sh.p
 *    p3dx.p
 *    peoplebot-sh.p
 *    perfpb+.p
 *    perfpb.p
 *    pion1m.p
 *    pion1x.p
 *    pionat.p
 *    powerbot-sh.p
 *    powerbot.p
 *    psos1m.p
 *    psos1x.p
*/

#ifndef _ROBOT_PARAMS_H
#define _ROBOT_PARAMS_H
#include <string>

void initialize_robot_params(void);


#define PLAYER_NUM_ROBOT_TYPES 29

// Default max speeds
#define MOTOR_DEF_MAX_SPEED 0.5
#define MOTOR_DEF_MAX_TURNSPEED DTOR(100)

/*
 * Apparently, newer kernel require a large value (200000) here.  It only
 * makes the initialization phase take a bit longer, and doesn't have any
 * impact on the speed at which packets are received from P2OS
 */
#define P2OS_CYCLETIME_USEC 200000

/* p2os constants */

#define P2OS_NOMINAL_VOLTAGE 12.0

/* Command numbers */
#define SYNC0 0
#define SYNC1 1
#define SYNC2 2

enum P2OSCommand {
  PULSE = 0,
  OPEN = 1,
  CLOSE = 2,
  ENABLE = 4,
  SETA = 5,
  SETV = 6,
  SETO = 7,
  MOVE = 8, ///< int, translational move (mm)
  ROTATE = 9, ///< int, set rotational velocity, duplicate of RVEL (deg/sec)
  SETRV = 10, ///< int, sets the maximum rotational velocity (deg/sec)
  VEL = 11,
  HEAD = 12, ///< int, turn to absolute heading 0-359 (degrees)
  DHEAD = 13, ///< int, turn relative to current heading (degrees)
  //DROTATE = 14, does not really exist
  SAY = 15, /**< string, makes the robot beep.
               up to 20 pairs of duration (20 ms incrs) and tones (halfcycle) */
  JOYINFO = 17, // int, requests joystick packet, 0 to stop, 1 for 1, 2 for
                // continuous
  CONFIG = 18, ///< int, request configuration packet
  ENCODER = 19, ///< int, > 0 to request continous stream of packets, 0 to stop
  RVEL = 21,
  DCHEAD = 22, ///< int, colbert relative heading setpoint (degrees)
  SETRA = 23,
  SONAR = 28,
  STOP = 29,
  DIGOUT = 30, ///< int, sets the digout lines
  //TIMER = 31, ... no clue about this one
  VEL2 = 32,
  GRIPPER = 33,
  //KICK = 34, um...
  ADSEL = 35, ///< int, select the port given as argument
  GRIPPERVAL = 36,
  GRIPPERPACREQUEST = 37, ///< p2 gripper packet request
  IOREQUEST = 40, ///< request iopackets from p2os
  PTUPOS = 41, ///< most-sig byte is port number, least-sig byte is pulse width
  TTY2 = 42,   // Added in AmigOS 1.2
  GETAUX = 43, // Added in AmigOS 1.2
  BUMP_STALL = 44,
  TCM2 = 45, ///< TCM2 module commands, see tcm2 manual for details
  JOYDRIVE = 47,
  MOVINGBLINK = 49, ///< int, 1 to blink lamp quickly before moving, 0 not to
                    ///(for patrolbot)
  HOSTBAUD = 50, ///< int, set baud rate for host port - 0=9600, 1=19200,
                 ///2=38400, 3=57600, 4=115200
  AUX1BAUD = 51, ///< int, set baud rate for Aux1 - 0=9600, 1=19200, 2=38400,
                 ///3=57600, 4=115200
  AUX2BAUD = 52, ///< int, set baud rate for Aux2 - 0=9600, 1=19200, 2=38400,
                 ///3=57600, 4=115200
  ESTOP = 55, ///< none, emergency stop, overrides decel
  ESTALL = 56, // ?
  GYRO = 58,         // Added in AROS 1.8
  // SRISIM specific:

  // for calibrating the compass:
  CALCOMP = 65, ///< int, commands for calibrating compass, see compass manual

  TTY3 = 66,   // Added in AmigOS 1.3
  GETAUX2 = 67,  // Added in AmigOS 1.3
  ARM_INFO = 70,
  ARM_STATUS = 71,
  ARM_INIT = 72,
  ARM_CHECK = 73,
  ARM_POWER = 74,
  ARM_HOME = 75,
  ARM_PARK = 76,
  ARM_POS = 77,
  ARM_SPEED = 78,
  ARM_STOP = 79,
  ARM_AUTOPARK = 80,
  ARM_GRIPPARK = 81,

  ROTKP = 82,        // Added in P2OS1.M
  ROTKV = 83,        // Added in P2OS1.M
  ROTKI = 84,        // Added in P2OS1.M
  TRANSKP = 85,      // Added in P2OS1.M
  TRANSKV = 86,      // Added in P2OS1.M
  TRANSKI = 87,      // Added in P2OS1.M
  SOUND = 90,
  PLAYLIST = 91,
  SOUNDTOG = 92, ///< int, AmigoBot (old H8 model) specific, enable(1) or
                 /// diable(0) sound
  // Power commands
  POWER_PC = 95,
  POWER_LRF = 96,
  POWER_5V = 97,
  POWER_12V = 98,
  POWER_24V = 98,
  POWER_AUX_PC = 125,
  POWER_TOUCHSCREEN = 126,
  POWER_PTZ = 127,
  POWER_AUDIO = 128,
  POWER_LRF2 = 129,

  // For SEEKUR or later lateral-capable robots
  LATVEL = 110, ///< int, sets the lateral velocity (mm)
  LATACCEL = 113, ///< int, sets the lateral acceleration (+, mm/sec2) or
                  /// lateral deceleration (-, mm/sec2)
  SETLATV = 0, ///< int, someday will set the vel
};

/* Server Information Packet (SIP) types */
#define STATUSSTOPPED 0x32
#define STATUSMOVING  0x33
#define ENCODER   0x90
#define SERAUX    0xB0
#define SERAUX2   0xB8  // Added in AmigOS 1.3
#define GYROPAC         0x98    // Added AROS 1.8
#define ARMPAC    160   // ARMpac
#define ARMINFOPAC  161   // ARMINFOpac
//#define PLAYLIST  0xD0

/* Argument types */
#define ARGINT    0x3B  // Positive int (LSB, MSB)
#define ARGNINT   0x1B  // Negative int (LSB, MSB)
#define ARGSTR    0x2B  // String (Note: 1st byte is length!!)

/* gripper stuff */
#define GRIPopen   1
#define GRIPclose  2
#define GRIPstop   3
#define LIFTup     4
#define LIFTdown   5
#define LIFTstop   6
#define GRIPstore  7
#define GRIPdeploy 8
#define GRIPhalt   15
#define GRIPpress  16
#define LIFTcarry  17

/* CMUcam stuff */
#define CMUCAM_IMAGE_WIDTH  80
#define CMUCAM_IMAGE_HEIGHT 143
#define CMUCAM_MESSAGE_LEN  10

/* conection stuff */
#define DEFAULT_P2OS_PORT "/dev/ttyS0"
#define DEFAULT_P2OS_TCP_REMOTE_HOST "localhost"
#define DEFAULT_P2OS_TCP_REMOTE_PORT 8101

/* degrees and radians */
#define DTOR(a) M_PI * a / 180.0
#define RTOD(a) 180.0 * a / M_PI

typedef struct
{
  double x;
  double y;
  double th;
  double length;
  double radius;
} bumper_def_t;

typedef struct
{
  double x;
  double y;
  double th;
} sonar_pose_t;


typedef struct
{
  double AngleConvFactor; //
  std::string Class;
  double DiffConvFactor; //
  double DistConvFactor; //
  int FrontBumpers; //
  double GyroScaler; //
  int HasMoveCommand; //
  int Holonomic; //
  int IRNum; //
  int IRUnit; //
  int LaserFlipped; //
  std::string LaserIgnore;
  std::string LaserPort;
  int LaserPossessed; //
  int LaserPowerControlled; //
  int LaserTh; //
  int LaserX; //
  int LaserY; //
  int MaxRVelocity; //
  int MaxVelocity; //
  int NewTableSensingIR; //
  int NumFrontBumpers; //
  int NumRearBumpers; //
  double RangeConvFactor; //
  int RearBumpers; //
  int RequestEncoderPackets; //
  int RequestIOPackets; //
  int RobotDiagonal; //
  int RobotLength; //
  int RobotRadius; //
  int RobotWidth; //
  int RotAccel; //
  int RotDecel; //
  int RotVelMax; //
  int SettableAccsDecs; //
  int SettableVelMaxes; //
  int SonarNum; //
  std::string Subclass;
  int SwitchToBaudRate; //
  int TableSensingIR; //
  int TransAccel; //
  int TransDecel; //
  int TransVelMax; //
  int Vel2Divisor; //
  double VelConvFactor; //
  sonar_pose_t sonar_pose[32];
  //bumper_def_t bumper_geom[32];
} RobotParams_t;


extern RobotParams_t PlayerRobotParams[];


#endif
